/*
 * Pid.c
 *
 *  Created on: 2025年9月16日
 *      Author: Move
 */

#include "zf_common_headfile.h"

//位置式PID
void PosPId_Updata(PID_T *p){

    /* 获取误差 */
    //Error1->上次误差，Error0->当前误差
    p->Error1 = p->Error0;


    //积分项
    if(p -> Ki != 0){
        p -> ErrorInt += p -> Error0;
    }
    else{
        p -> ErrorInt = 0;
    }
    //积分限幅
    if (p->ErrorInt > p->MAXInt) {p->ErrorInt = p->MAXInt;}
    if (p->ErrorInt < -p->MAXInt) {p->ErrorInt = -p->MAXInt;}

    //PID公式
    p->Out   = p->kp1 * p->Error0 *p->Error0
             + p->Kp * p->Error0
             + p->Ki * p->ErrorInt
             + p->Kd * (p->Error0 - p->Error1);
    //输出限幅
    if(p->Out > p->OutMax){
        p->Out = p->OutMax;
    }
    if(p->Out < p->OutMin){
        p->Out = p->OutMin;
    }

    p->Error0 = p->Target - p->Actual;
}

//增量式PID
void InsPId_Updata(PID_T *p){
    //Error2->上上次误差 Error1->上次误差，Error0->当前误差
    p->Error0 = p->Target - p->Actual;

    p->Out +=(int)((p->Kp) * ((p->Error0) - (p->Error1)))
           +      ((p->Ki) * (p->Error0))
           +      ((p->Kd) * ((p->Error0) - 2 * (p->Error1) + (p->Error2)));

    //输出限幅
    if(p->Out > p->OutMax){
        p->Out = p->OutMax;
    }
    if(p->Out < p->OutMin){
        p->Out = p->OutMin;
    }

    p->Error2 = p->Error1;
    p->Error1 = p->Error0;
}
